//*****************************************************************
// PIDTransformThread.cpp
//
//  Created on: Jan 21, 2012
//      Author: Christopher Wood
//              Robert Livosi
//
//  $Id: PIDTransformThread.cpp 23 2012-01-28 02:51:05Z caw4567@gmail.com $
//*****************************************************************

#include "PIDTransformThread.h"

/**
 * The (friendly) run function for the transform thread.
 *
 * @param arg - a pointer to the host PIDTransformThread object.
 */
void* transformThreadRun(void* arg);

/**
 * Create a new PID transform thread with the specified I/O group.
 *
 * @param ioGroup - analog I/O thread object.
 */
PIDTransformThread::PIDTransformThread(IOThreadGroup* signalGroup)
	: TransformThread(signalGroup)
{
	// Initialize all transform variables and buffers
	uIndex = 0;
	eIndex = 0;
	memset(uBuffer, 0, sizeof(float) * OUTPUT_SIZE);
	memset(eBuffer, 0, sizeof(float) * OUTPUT_SIZE);

	// Initialize the PID parameters to some good values
	this->kp = 1.0;
	this->ki = 0.0;
	this->kd = 0.0;
	this->reference = 2.5;

	// Tell the signal group we are the transform function to use
	signalGroup->assignTransform(this);

	// Create the input/output execution semaphores
	sem_init(&transformSem, 0, 0);
}

/**
 * Default destructor.
 */
PIDTransformThread::~PIDTransformThread()
{
}

/**
 * Start the thread.
 */
void PIDTransformThread::run()
{
	setThreadState(true);
	pthread_create(&transformThreadID, NULL, transformThreadRun, this);
}

/**
 * The (friendly) run function for the transform thread.
 *
 * @param arg - a pointer to the host PIDTransformThread object.
 */
void* transformThreadRun(void* arg)
{
	PIDTransformThread* transformer = (PIDTransformThread*)arg;
	float error = 0.0;
	float value = 0.0;
	float kp = 0.0;
	float ki = 0.0;
	float kd = 0.0;
	float* uBuffer = transformer->getUBuffer();
	float* eBuffer = transformer->getEBuffer();
	int8_t uIndex = 0;
	int8_t eIndex = 0;

	while (transformer->isAlive())
	{
		// Wait until we have data to process
		sem_wait(transformer->getTransformSem());
		if (!transformer->isAlive())
		{
			break;
		}

		// Get the latest input for the transform
		value = (transformer->getSignalGroup())->getInputValue();

		// Calculate error
		error = transformer->getReference() - value;

		// Perform the conversion
		kp = transformer->getKp();
		kd = transformer->getKd();
		ki = transformer->getKi();
		uIndex = transformer->getUIndex();
		eIndex = transformer->getEIndex();
		uBuffer[uIndex] = ((((kp + ki + kd) * error) -
				(kp + (2 * kd)) * eBuffer[(eIndex + PIDTransformThread::OUTPUT_SIZE - 1) % PIDTransformThread::OUTPUT_SIZE] +
				(kd * eBuffer[(eIndex + PIDTransformThread::OUTPUT_SIZE - 2) % PIDTransformThread::OUTPUT_SIZE]))) +
				uBuffer[(uIndex + PIDTransformThread::OUTPUT_SIZE - 1) % PIDTransformThread::OUTPUT_SIZE];

		// Check for and handle clipping
		if (uBuffer[uIndex] > UPPER_VOLTAGE_CLIP)
		{
			uBuffer[uIndex] = UPPER_VOLTAGE_CLIP;
		}
		else if (uBuffer[uIndex] < LOWER_VOLTAGE_CLIP)
		{
			uBuffer[uIndex] = LOWER_VOLTAGE_CLIP;
		}

		// Output to the analog output thread
		(transformer->getSignalGroup())->setOutputValue(uBuffer[uIndex]);
		sem_post((transformer->getSignalGroup())->getOutputThreadSem());

		// Update history indices
		eBuffer[eIndex] = error;
		transformer->advanceUIndex();
		transformer->advanceEIndex();
	}

	return NULL;
}

/**
 * Retrieve the current output index.
 *
 * @return uIndex
 */
int8_t PIDTransformThread::getUIndex()
{
	return uIndex;
}

/**
 * Retrieve the current error index.
 *
 * @return eIndex
 */
int8_t PIDTransformThread::getEIndex()
{
	return eIndex;
}

/**
 * Advance the output index.
 */
void PIDTransformThread::advanceUIndex()
{
	uIndex = (uIndex + 1) % OUTPUT_SIZE;
}

/**
 * Advance the error index.
 */
void PIDTransformThread::advanceEIndex()
{
	eIndex = (eIndex + 1) % OUTPUT_SIZE;
}

/**
 * Retrieve a pointer to the output history buffer.
 *
 * @return output history buffer
 */
float* PIDTransformThread::getUBuffer()
{
	return uBuffer;
}

/**
 * Retrieve a pointer to the error history buffer.
 *
 * @return error history buffer
 */
float* PIDTransformThread::getEBuffer()
{
	return eBuffer;
}

/**
 * Set the Kp value.
 *
 * @param kp - new Kp value.
 */
void PIDTransformThread::setKp(float kp)
{
	this->kp = kp;
}

/**
 * Set the Ki value.
 *
 * @param ki - new Ki value.
 */
void PIDTransformThread::setKi(float ki)
{
	this->ki = ki;
}

/**
 * Set the Kd value.
 *
 * @param kd - new Kd value.
 */
void PIDTransformThread::setKd(float kd)
{
	this->kd = kd;
}

/**
 * Set the new reference value (set point).
 *
 * @param ref - the new reference value.
 */
void PIDTransformThread::setReference(float ref)
{
	this->reference = ref;
}

/**
 * Retrieve the Kp parameter.
 *
 * @return kp;
 */
float PIDTransformThread::getKp()
{
	return kp;
}

/**
 * Retrieve the Ki parameter.
 *
 * @return ki;
 */
float PIDTransformThread::getKi()
{
	return ki;
}

/**
 * Retrieve the Kd parameter.
 *
 * @return kd;
 */
float PIDTransformThread::getKd()
{
	return kd;
}

/**
 * Retrieve the reference parameter.
 *
 * @return reference;
 */
float PIDTransformThread::getReference()
{
	return reference;
}
